/*
 * motor_dirver.c
 *
 *  Created on: 2021年10月31日
 *      Author: 王慧云
 */


#include "motor_dirver.h"

#define  ENCODER_TOTAL_RESOLUTION  (encoder_resolution * 4) //编码器倍频

uint16_t motor_freq = 0;     //电机的频率
uint16_t motor_dutyCycle = 0;//电机的占空比
uint16_t motor_maxSpeed = 210; //电机最大速度
uint8_t motor_runStatus = 0; //电机运行模式 0--未运行 1--运行  2--故障
bool motor_runDir = 0;   //电机运行模式 0--正转  1--反转

uint16_t encoder_resolution = 13; //编码器分辨率
uint16_t reduction_ratio = 30; //减速比
float motor_speed; //电机速度

uint8_t motor_CtrlMode = KNOB;/*电机调节方式设置*/

int io_speed1 = 0; //在IO模式下，速度1
int io_speed2 = 0; //在IO模式下，速度2
int io_speed3 = 0; //在IO模式下，速度3
int io_speed4 = 0; //在IO模式下，速度4

//bool motor_isSpeedInput = 1; //判断是否是以转速方式调节的
bool pid_enable = 1; //pid控制使能 0--不使能 1--使能
//定义pid控制器变量
_pid pid = {0,0,0,0,13,3.5,0.04};

/**
 * 从重新设置输出频率
 * 设置范围1kHz - 25kHz
 *
 */
void motor_set_freq(uint16_t freq)
{
	htim3.Init.Prescaler = 84 / freq -1;
	if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
	{
		printf("tim Error\r\n");
		Error_Handler();
	}
	motor_freq = freq;
}

/**
 * 设置占空比以及方向
 * 输入占空比0-100，输入方向 0--正转 1--反转
 */
void motor_set_dutyCycle(uint16_t duty,bool dir)
{
	if(dir == CW)
	{
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,duty);
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,0);
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,0);
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,duty);
	}
	motor_dutyCycle = duty;
	motor_runDir = dir;
}

/*设置电机速度*/
void motor_set_speed(int16_t speed)
{
	if(speed < 0)
	{
		speed = -speed;
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,0);
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,speed);
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,speed);
		__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,0);		
	}
}


static void motor_Start()
{
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
}

static void motor_Stop()
{
	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_3);
	HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_4);
}


/**
 * 函数功能，设置电机启停
 */
void motor_set_RunStatus(uint8_t status)
{
	switch(status)
	{
		case 0:
			motor_Stop();
			break;
		case 1:
			motor_Start();
			break;
		case 2:
			motor_Start();
			break;
	}
	motor_runStatus = status;
}

/****************电机控制部分************************/
/***电机速度采样***/
/**
 * 此函数需要在一个特定周期内调用
 * 传入参数为调用此函数的周期单位为 ms
 * 建议可以被60000整除
*/
void motor_speed_tick(uint16_t tick_period)
{
	uint8_t dir = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim1);//获取方向
	//static uint16_t Last_Count = 0;
	uint16_t time_Factor = 60000/tick_period;
	/* 当前时刻总计数值 = 计数器值 + 计数溢出次数 * 溢出值  */
	uint16_t Capture_Count = __HAL_TIM_GET_COUNTER(&htim1);
	// M法测速
	motor_speed = time_Factor * (float)(Capture_Count - 34767) /(ENCODER_TOTAL_RESOLUTION); 
	motor_speed = motor_speed/(float)reduction_ratio;
	__HAL_TIM_SET_COUNTER(&htim1,34767);
}


/*计算pid值 位置式**/
//也可将此替换为增量式
float PID_realize(float actual_val)
{
	static float intergral = 0;//累计误差
	/*计算目标值与实际值的误差*/
	pid.err = pid.target_val - actual_val;
	/*计算累计误差*/
	intergral += pid.err;
	/*PID算法实现*/
	actual_val = pid.Kp*pid.err + pid.Ki*intergral + pid.Kd*(pid.err - pid.err_last);
	/*误差传递*/
	pid.err_last = pid.err;
	/*返回当前实际值*/
	return actual_val;
}


/***IO控制相关函数**/
void motor_io_set_speed(uint8_t mode)
{
	int16_t speed = 0;
	switch (mode)
	{
		case 0:
			speed = io_speed1;
			break;
		case 1:
			speed = io_speed1;
			break;
		case 2:
			speed = io_speed1;
			break;	
		case 3:
			speed = io_speed1;
			break;	
		default:
			break;
	}
	if(!motor_IsPIDenable())
	{
		motor_set_dutyCycle(speed,motor_get_runDir());
	}
	else
	{
		pid.target_val = speed;
	}

}


